'''
Created on Feb 23, 2014

@author: yfrank
'''
import CodingUtils.GeneralUtils as GeneralUtils

import CodingUtils.Logging as Logging

import subprocess , threading as Thread

import Transport.Enums as Enums

import time


#from CodingUtils.Geometry import Polygon as Polygon

from CodingUtils.Geometry import parse_robots as parse_robots


import os , linecache , math
from shutil import copyfile
from math import sin
from math import cos

#DLLPATH = "..\\..\\PathPlanning\\RRT_rotating_poly\\MRMPApplication\\"
DLLPATH = "..\\..\\PathPlanning\\RRT_two\\MRMPApplication\\"
RRTPATH = "..\\MRMPApplication.exe" #relative to DLLPATH
OPTPATH = "..\\..\\PathOPT\\Release\\PathOPT.exe" #relative to DLLPATH
WORKSPACE = ".\\"
OUTPUTFILE = 'path_opt.txt'


class TimedProcess (subprocess.Popen):
    def __init__(self,args,name,shell=False):
                super(TimedProcess,self).__init__(args,shell)
                #print "SCRIPT-INFO: Running ",name
                self.name = name
                self.t = None

    def set_to_stop(self,secs):
        self.t = Thread.Timer(float(secs),self.timeout_expired)
        self.t.start()

    def timeout_expired(self):
        if self.isRunning():
            Logging.log("SCRIPT-INFO: Stopping "+self.name)
            self.terminate()

    def stop(self):
        if self.isRunning():
            if self.t: self.t.cancel()
            self.terminate()

    def __del__(self):
        if self.t:
            self.t.cancel()

    def isRunning(self):
        return self.poll() == None


class PathFunctions(object):
    '''
    classdocs
    '''

    def __init__(self,peer):
        self.sim_peer = peer

    def notify_success(self,data):
        self.sim_peer.handle_event(Enums.PeerEvent.SUCC,data)

    def notify_failure(self,data):
        self.sim_peer.handle_event(Enums.PeerEvent.FAIL,data)

    def do_calculate_path(self,inputdir): #TODO replace with...
        #output_file = solveS(WORKSPACE,RRTPATH,self.inputdir)
        try:
            data = solveS(RRTPATH,OPTPATH,inputdir)
            self.notify_success(data)
        except Exception, e:
            Logging.log(str(e))
            self.notify_failure(e)


def addRobotWorkspace(fdPath,offset,robotNumber,inOut):
    """
    addRobotWorkspace:
        add to the workspace a single robot
        fdPath - folder path
        inOut - if we want to move the robot to source point inOut = "source"
                else inOut = "target"
        robotNumber - the number of the wanted robot
    """
    assert(robotNumber>0)
    workspaceFd = open(fdPath+"workspace_A.txt","r+")

    wsLines = workspaceFd.readlines()
    workspaceFd.seek(0)

    workspaceFd.write(str(int(wsLines[0].strip())+1)+'\n')
    for i in range(1,len(wsLines)):
        if i == offset+robotNumber:
            workspaceFd.write(moveRobot(fdPath,robotNumber,inOut))
        workspaceFd.write(wsLines[i])
    #if the robot was the last in the file
    if len(wsLines) == offset+robotNumber:
        workspaceFd.write(moveRobot(fdPath,robotNumber,inOut))
    workspaceFd.close()

def removeFromWorkspace(fdPath,robotNumber):
    """
    removeFromWorkspace:
        remove from the workspace a single robot
        fdPath - folder path
        robotNumber - the number of the wanted robot
    """
    assert(robotNumber>0)
    workspaceFd = open(fdPath+"workspace_A.txt","r+")
    wsLines = workspaceFd.readlines()

    #clear the contant of the file
    eraseContent(workspaceFd)
    #printing the file except for the specific robot
    workspaceFd.write(str(int(wsLines[0].strip())-1)+'\n')
    for i in range(1,len(wsLines)):
        if i != robotNumber:
            workspaceFd.write(wsLines[i])
    workspaceFd.close()

def buildWorkspace(fdPath):
    """
    buildWorkspace:
        add the robots as obstacle to the workspace
        fdPath - folder path
    """
    robotsFd = open(fdPath+"robot_A.txt","r")
    workspaceFd = open(fdPath+"workspace_A.txt","r+")

    roboN = int(robotsFd.readline().strip())
    robotsFd.close()
    obsN = int(workspaceFd.readline().strip())+roboN
    #number of obstacle in the workspace

    workspaceFd.seek(0)
    workspaceFd.write(str(obsN)+'\n')

    workspaceFd.seek(0,2)
    for i in range(1,roboN+1):
        workspaceFd.write(moveRobot(fdPath,i,"target"))

    workspaceFd.close()

def movePolygon(coordinates,target):
#assuming the robot coordinates are around zero
    """
    movePolygon:
        move the polygon from (0,0) to target
        coordinates - the original polygon coordinates
        target - the centre of the target coordinates
    """
    coordinates = coordinates.strip()
    target = target.strip().split()

    nCord = int(coordinates[:coordinates.index(' ')])
    nPoly = str(nCord)
    coordinates = coordinates[coordinates.index(' ')+1:].split()

    dx = float(target[0])
    dy = float(target[1])
    ang = (float(target[2])*math.pi)/180

    for i in range(0,nCord):
        x = float(coordinates[2*i])
        y = float(coordinates[2*i+1])

        newX=x*cos(ang)-y*sin(ang)
        newY=x*sin(ang)+y*cos(ang)
        newX+=dx
        newY+=dy
        # can change the turnc if necessary
        nPoly += " %.12f %.12f" % (newX , newY)
    return nPoly

def movePolygonToZero(coordinates,source):
    """
    movePolygonToZero:
        move the polygon from source to (0,0)
        coordinates - the original polygon coordinates
        source - the centre of the source coordinates
    """
    coordinates = coordinates.strip()
    source = source.strip().split()

    nPoly = coordinates[:coordinates.index(' ')]
    nCord = int(nPoly)
    coordinates = coordinates[coordinates.index(' ')+1:].split()

    dx = float(source[0])
    dy = float(source[1])
    ang = (float(source[2])*math.pi)/180
    #XXX does the angle is in radins or deg
    for i in range(0,nCord):
        x = float(coordinates[2*i])
        y = float(coordinates[2*i+1])

        newX = x-dx
        newY = y-dy
        newXA=newX*cos(-ang)-newY*sin(-ang)
        newYA=newX*sin(-ang)+newY*cos(-ang)

        # can change the turnc if necessary
        nPoly += " %.12f %.12f" % (newXA , newYA)
    return nPoly

def moveRobot(fdPath,robotNumber,inOut):
    """
    moveRobot:
        move the robot from (0,0) to source or target
        fdPath - folder path
        inOut - if we want to move the robot to source point inOut = "source"
                else inOut = "target"
        robotNumber - the number of the wanted robot
    """
    assert(robotNumber>0)
    robot = getLineFromFile(fdPath,robotNumber,"robot_A.txt")
    if inOut == "source":
        robot = movePolygon(robot,getLineFromFile(fdPath,robotNumber,"source.txt"))
    else:
        robot = movePolygon(robot,getLineFromFile(fdPath,robotNumber,"target.txt"))
    return robot+'\n'

def changeFileToLines(fdPath,fileName,newFileName):
    """
    changeFileToLines:
        chnage the file so each polygon will be describe in one line
        fdPath - folder path
        fileName - the file we read from
        newFileName - the file we write to
    """
    assert(fileName != newFileName)
    robotFd = open(fdPath+fileName,"r")
    newRobotFd = open(fdPath+newFileName,"w")
    newRobotFd.write(robotFd.readline().strip()+'\n')

    while True:
        line = robotFd.readline()
        if not line:
            break

        robot= line.strip()
        nVertices = int(robot)

    #for every robot read and write all the coordinates
        for i in range(0,nVertices):
            robot = robot + ' ' + robotFd.readline().strip()

        newRobotFd.write(robot+'\n')
    robotFd.close()
    newRobotFd.close()

def buildQuery(fdPath,robotNumber):
    """
    buildQuery:
        build the query file from source and target files
        fdPath - folder path
        robotNumber - the number of the wanted robot
    """
    assert(robotNumber>0)
    queryFd = open(fdPath+"query.txt","w")
    #XXX check what line we need to get is it start from 0 or 1
    #we flip the source and the target
    source  = linecache.getline(fdPath+"source.txt", robotNumber+1).strip()
    target = linecache.getline(fdPath+"target.txt", robotNumber+1).strip()

    eraseContent(queryFd)

    queryFd.write("\n1\n\n2\n")
    queryFd.write(target+" "+source+"\n")

    queryFd.close()
    linecache.clearcache()

def buildRobot(fdPath,robotNumber):
    """
    buildRobot:
        build the robot coordinate file
        fdPath - folder path
        robotNumber - the number of the wanted robot
    """
    assert(robotNumber>0)
    robotFd = open(fdPath+"robot_S.txt","w")
    #XXX check what line we need to get is it start from 0 or 1
    robot  = linecache.getline(fdPath+"robot_A.txt", robotNumber+1)
    linecache.clearcache()

    eraseContent(robotFd)

    robotFd.write("\n1\n")
    robotFd.write(robot)

    robotFd.close()

def buildAllRobots(fdPath):
    """
    buildAllRobots:
        build the robots coordinate file
        fdPath - folder path
    """
    changeFileToLines(fdPath,"robot.txt","robot_temp.txt");
    robotFd = open(fdPath+"robot_temp.txt","r")
    newRobotsFd = open(fdPath+"robot_A.txt","w")

    line = robotFd.readline()
    newRobotsFd.write(line)
    robotNumber=1

    for line in robotFd:
        newRobotsFd.write(movePolygonToZero(line,getLineFromFile(fdPath,robotNumber,"target.txt"))+'\n')
        robotNumber+=1

    robotFd.close()
    newRobotsFd.close()
    os.remove(fdPath+"robot_temp.txt")

def getLineFromFile(fdPath,n,fileName):
    """
    getLineFromFile:
        return: line number n from file
        fdPath - folder path
        n - the number of the wanted line
        file - name of the file
    """
    assert(n>=0)
    line =  linecache.getline(fdPath+fileName, n+1)
    linecache.clearcache()
    return line

def flipRoute(routePath,output_folder,pathNumber,robotId,ifname,ofname):
    """
    flipRoute:
        flip the path from beginning to end
        routePath - the full path of the path file
		pathNumber - robot id
    """
    pathFd = open(routePath+pathNumber+"\\"+ifname,"r")

    #flip the path
    lines = pathFd.readlines()[::-1]
    lines.pop()
    robotNumber = int(robotId)
    newPathFd = open(routePath+output_folder+ofname,"w")
    newPathFd.write("id "+robotId+'\n')

    targetCoor = getLineFromFile(routePath,robotNumber,"target.txt").strip()
    for i in range(0,len(lines)):
        if lines[i].startswith('a'):
            #decreasing 180 degrees from the angle
            newPathFd.write("angle %.12f\n" % (float(lines[i][lines[i].index(' ')+1:].strip())*-1))
        else:
            for j in range(i+1,len(lines)):
                foundL=False
                if lines[j].startswith('l'):
                    newPathFd.write(lines[j])
                    foundL = True
                    break
            if not foundL:
                    newPathFd.write("line " +targetCoor[:targetCoor.rindex(' ')]+ '\n')

    #add the target x y
    #need to receive folder of target

    pathFd.close()
    newPathFd.close()


def eraseContent(filePath):
    """
    eraseContent:
        erase the file content
        file - file descriptor
    """
    filePath.seek(0)
    filePath.truncate()
    filePath.seek(0)

def copy_files(from_dir,to_dir):
    """
    copy the workspace , robot , query file from from_dir to to_dir.
        from_dir - source folder
        to_dir - target folder
    """
    copyfile(from_dir+"query.txt", to_dir+"query.txt")
    copyfile(from_dir+"workspace.txt", to_dir+"obstacles.txt")
    copyfile(from_dir+"workspace_A.txt", to_dir+"workspace_A.txt")
    copyfile(from_dir+"robot_S.txt", to_dir+"robot_S.txt")

def optimize_path(opt_exepath,dirname,robot_file,workspace_file,robot_id,num_path):
    args = opt_exepath+" "+dirname+" "+robot_file+" "+workspace_file+" "+str(robot_id)+" "+str(num_path)
    return os.system(args)

def solveS(rrt_path,opt_pth,dirname,numOfPaths=2,bOptimize=1):

    local_dir = os.getcwd()
    workspace = dirname+'\\'
    os.chdir(DLLPATH)
    nRobot = int(getLineFromFile(workspace,0,"robot.txt").strip())
    toatl_robot = nRobot

    robots = parse_robots(workspace+"robot.txt")

    nObstaculos = int(getLineFromFile(workspace,0,"workspace.txt").strip())

    robList = [i+1 for i in range(0,nRobot)]
    currRobot = 0
    pathFound = 1

    buildAllRobots(workspace)
    changeFileToLines(workspace,"workspace.txt","workspace_A.txt")

    buildWorkspace(workspace)

    while robList:
        robot = robots[currRobot]
        robot_id = str(robList[currRobot])
        robot_folder_id = str(toatl_robot + 1 - robList[currRobot])
        curr_folder =workspace+robot_folder_id+"\\"
        GeneralUtils.ensure_dir(curr_folder)
        buildQuery(workspace,robList[currRobot])
        buildRobot(workspace,robList[currRobot])
        removeFromWorkspace(workspace,nObstaculos+robList[currRobot])

        copy_files(workspace,curr_folder)

        f = open(curr_folder+"Robot.txt","w+")
        f.write(str(robot))
        f.close()

        num_path = 0
        while(num_path<numOfPaths): #run RRT until enough paths are found
            relative_folder = os.path.relpath(curr_folder)+'\\'
            result = os.system(rrt_path+" "+str(num_path)+" "+relative_folder)
            if result == 1: num_path+=1
            msg = "RRT_RESULT_CODE : "+str(result)
            Logging.log(msg)

        result = 0
        if bOptimize==1: #call to optimizer
            result = optimize_path(opt_pth,relative_folder,"Robot.txt","obstacles.txt",robot_id,num_path)
        else:
            copyfile(workspace+robot_folder_id+"\\"+"path_robot.txt", workspace+robot_folder_id+"\\"+"path_opt.txt")
            result = 1
        msg = "OPT_RESULT_CODE : "+str(result)
        Logging.log(msg)
        if result==1:
        # the path title is the number of path found
            flipRoute(workspace,'',robot_folder_id,robot_id,"path_opt.txt","path_"+robot_folder_id+".txt")
            flipRoute(workspace,robot_folder_id+"\\",robot_folder_id,robot_id,"path_robot_imp.txt","f_path_robot_imp.txt")
            pathFound+=1;
            addRobotWorkspace(workspace,nObstaculos,robList[currRobot],"source")
            del(robList[currRobot])
            nRobot -= 1
            if currRobot != 0:
                currRobot = currRobot % nRobot
        else:
            addRobotWorkspace(workspace,nObstaculos,robList[currRobot],"target")
            currRobot = (currRobot+1) % nRobot

    os.chdir(local_dir)
    return pathFound-1



